// Debug process: the first "TRACE", Ln 1064

#include <time.h>
#include <limits.h>
#include "ddmcmc_meanshift.h"

using namespace cv;
using namespace std;

////////////////////////////////////////////////////////////////////////
//ddmcmc_meanshift.cpp
//This file segments regions from color image using Mean Shift algorithm.
//
// copy from Rutgers University
//  modified by Zhuowen Tu, Siyuan Qi
////////////////////////////////////////////////////////////////////////

// Moved from header file
static const double     Un_prime = (double)0.1978;
static const double     Vn_prime = (double)0.4683;
static const double     Lt = (double)0.008856;

// Constants for LUV transformation
static const double     Xn = (double)0.9505;
static const double     Yn = (double)1.0;
static const double     Zn = (double)1.0888;

static const int FIRST_SIZE = 262144; // 2^18
static const int SEC_SIZE = 64;  // 2^6

static const int RGB_LUV_XYZ[3][3] = { { 4125, 3576, 1804 },
{ 2125, 7154, 721 },
{ 193, 1192, 9502 } };
static const int MAXV = 256;

static const int rect_gen_contor = 3;

// Radius of the searching window
static double gen_RADIUS[3] = { 2, 3, 4 };
static double rect_RADIUS[rect_gen_contor] = { 8, 6, 4 };
static double fix_RADIUS[rect_gen_contor];
static double final_RADIUS;
static double RADIUS2;
static double RADIUS;

static double max_dist;

static int my_threshold[3] = { 50, 100, 350 };
static int my_rap[3] = { 4, 2, 1 };
static int act_threshold;

#define my_abs(a) ((a) > 0 ? (a): (-a))
#define SQRT2 1.4142
#define SQRT3 1.7321
static const double	BIG_NUM = 1.0e+20;

// # of samples
static const int	Max_J = 25;

// Limit of the number of failed trials
static const int       Max_trials = 50;

// Defaults values for the parameters.
static const int	sam_max = 60;

// Few more trials at the end
static const int        MAX_TRIAL = 10;

// Make coordinate computation faster
// my_neigh is for auto_segm, my_neigh_r works with region
static int my_neigh[8];
static int my_neigh_r[8];


// Results
static int ORIG_COLORS;
static int SEGM_COLORS;

//==========================================================
// Class : irmm_SegMs
//==========================================================
////////////////////////////////////////////////////////////
// Class constructor
DDMCMC_MeanShiftClustering::DDMCMC_MeanShiftClustering(Mat mOriginalImg)
{
	// SIYUAN

	mImg_RGB = mOriginalImg.clone();
	nRows = mImg_RGB.rows;
	nCols = mImg_RGB.cols;
	count = nRows*nCols;
	mImg_RGB.convertTo(mImg_RGB, CV_8UC3);
	mImg_LUV = Mat(nRows, nCols, CV_16SC3);
	/*
	mImg_BGR = mOriginalImg.clone();
	nRows = mImg_BGR.rows;
	nCols = mImg_BGR.cols;
	count = nRows*nCols;
	mImg_BGR.convertTo(mImg_BGR, CV_8UC3);
	mImg_LUV = Mat(nRows, nCols, CV_16SC3);
	*/
	//cvtColor(mImg_BGR, mImg_LUV, CV_BGR2Luv);
	//mImg_LUV.convertTo(mImg_LUV, CV_8UC3);

	_k = 0;
	_comp_cnt = 0;
	_rg_rows = 0;
	_rg_colms = 0;
	_data = nil;
	_max_sub_samp = Max_J;
	option = 2;
}

// Class destructor
DDMCMC_MeanShiftClustering::~DDMCMC_MeanShiftClustering()
{
	if (_data)
	{
		for (int i = 0; i < _k; i++)
		if (_data[i])
			delete[] _data[i];
		delete[] _data;	_data = NULL;
	}
}

// Update _col_remain[], _m_col_remain, and _n_col_remain
void DDMCMC_MeanShiftClustering::my_actual(uchar *my_class)
{
	int i;
	int temp_contor = _remn_cnt;
	int *temp_rem = new int[count];
	memcpy(temp_rem, _remn_cnt_map, sizeof(int)*temp_contor);
	_remn_cnt = 0;
	for (i = 0; i < temp_contor; i++)
	if (!my_class[temp_rem[i]])
		_remn_cnt_map[_remn_cnt++] = temp_rem[i];
	delete[] temp_rem;
	memset(_col_remain, 0, sizeof(int)*_n_col_remain);
	memset(_m_col_remain, 0, sizeof(int)*_n_colors);
	_n_col_remain = 0;
	for (i = 0; i < _remn_cnt; i++)
		_m_col_remain[_col_index[_remn_cnt_map[i]]]++;
	for (i = 0; i < _n_colors; i++)
	if (_m_col_remain[i])
	{
		_col_remain[_n_col_remain] = i;
		_n_col_remain++;
	}
}

// if more than "how_many"  neighbors, consider the point
void DDMCMC_MeanShiftClustering::test_neigh(uchar* my_class, int *selected, int* my_contor, int how_many)
{
	int i, j, p, k;
	uchar* local_class = my_class;
	int temp_contor = *my_contor;
	int my_index;

	if (auto_segm) my_index = _remn_cnt;
	else          my_index = count;
	for (p = 0, i; p < my_index; p++)
	{
		if (auto_segm) i = _remn_cnt_map[p];
		else          i = p;
		if (!local_class[i])
		{
			int neigh_contor = 0, no_neigh = 1;
			for (j = 0; j < 8; j++)
			{
				k = i + my_neigh[j];
				if (k >= 0 && k < count && local_class[k])
				{
					if (auto_segm && _labels[k] != 255) continue;
					neigh_contor++;
					if (neigh_contor > how_many)
					{
						no_neigh = 0;
						break;
					}
				}
			}
			if (!no_neigh)
			{
				if (auto_segm) selected[*my_contor] = i;
				*my_contor = *my_contor + 1;
			}
		}
	}
	for (i = temp_contor; i < *my_contor; i++)
		local_class[selected[i]] = 1;
}


// Find the feature vectors inside the given window
// Use Improved Absolute Error Inequality Criterion
// when computing Euclidean distance
// See J.S.Pan el al, Fast Clustering Alg. for VQ, Pattern Recognition,
// Vol. 29, No. 3, pp. 511-518, 1996

void DDMCMC_MeanShiftClustering::new_auto_loop(double *final_T, uchar *sel_col)
{
	double	fL, fU, fV, RAD2, R;
	int		TT0 = 0, TT1 = 0, TT2 = 0;
	int		local_contor = 0;
	double	final_T0 = final_T[0], final_T1 = final_T[1], final_T2 = final_T[2];
	double	RADIUS_S2 = SQRT2*RADIUS, RADIUS_S3 = SQRT3*RADIUS;

	for (int p = 0, k; p < _n_col_remain; p++)
	{
		k = _col_remain[p];
		fL = _col0[k] - final_T0; if ((fL = my_abs(fL)) >= RADIUS) continue;
		fU = _col1[k] - final_T1; if ((R = my_abs(fU) + fL) >= RADIUS_S2) continue;
		fV = _col2[k] - final_T2; if (R + my_abs(fV) >= RADIUS_S3) continue;
		RAD2 = fL * fL + fU * fU + fV * fV;
		if (RAD2 < RADIUS2)
		{
			int r = _m_col_remain[k];
			TT0 += _col0[k] * r; TT1 += _col1[k] * r; TT2 += _col2[k] * r;
			local_contor += r;
			sel_col[k] = 1;
		}
	}
	final_T[0] = (double)TT0 / (double)local_contor;
	final_T[1] = (double)TT1 / (double)local_contor;
	final_T[2] = (double)TT2 / (double)local_contor;
}

// The same as above, but for non auto_segmentation
void DDMCMC_MeanShiftClustering::nauto_loop(double *final_T, int *selected, uchar *my_class, int *my_contor)
{
	double	fL, fU, fV, RAD2, R;
	int		local_contor = *my_contor;
	double	final_T0 = final_T[0], final_T1 = final_T[1], final_T2 = final_T[2];
	double	RADIUS_S2 = SQRT2*RADIUS, RADIUS_S3 = SQRT3*RADIUS;

	for (int k = 0; k < _rg_cnt; k++)
	{
		fL = _data[0][k] - final_T0; if ((fL = my_abs(fL)) >= RADIUS)  continue;
		fU = _data[1][k] - final_T1; if ((R = my_abs(fU) + fL) >= RADIUS_S2)  continue;
		fV = _data[2][k] - final_T2; if (R + my_abs(fV) >= RADIUS_S3)  continue;
		RAD2 = fL * fL + fU * fU + fV * fV;
		if (RAD2 < RADIUS2)
		{
			selected[local_contor++] = k;
			my_class[k] = 1;
		}
	}
	*my_contor = local_contor;
}

// Set the Radius of the window
void DDMCMC_MeanShiftClustering::set_RADIUS(int color_cnt, int final)
{
	if (final == 2) RADIUS = final_RADIUS*1.26;
	else if (final == 1) RADIUS = final_RADIUS;
	else RADIUS = fix_RADIUS[color_cnt];
	RADIUS2 = RADIUS * RADIUS;
}

// Test if the clusters have the same mean
int DDMCMC_MeanShiftClustering::test_same_cluster(int rect, double T[][p_max])
{
	double RAD2;

	// SIYUAN modified
	for (int r = 0; r < nRows; r++){
		for (int c = 0; c < nCols; c++){
			RAD2 = color_dis(r, c, T[rect][0], T[rect][1], T[rect][2]);
			if (RAD2 < 1)
				return 1;
		}
	}

	return 0;
}

// First take only pixels inside the search windows at their final locations
// Then inflate windows to double volume and retain only pixels which are
// neighbors with the previous
void DDMCMC_MeanShiftClustering::get_codeblock1(double T[][p_max], int n_rects)
{
	double	fL, fU, fV, RAD2, min_RAD2;
	int		min_ind;
	int		i, k, u;
	int		pres_class;
	double	*pLUV;

	if (auto_segm)
		set_RADIUS(0, 0);
	else
		set_RADIUS(2, 0);

	// SIYUAN modified
	k = 0;
	for (int r = 0; r < nRows; r++){
		for (int c = 0; c < nCols; c++){
			Vec3s color = mImg_LUV.at<Vec3s>(r, c);
			min_RAD2 = BIG_NUM;
			min_ind = 0;
			for (i = 0; i < n_rects; i++)
			{
				pLUV = T[i];
				fL = color.val[0] - pLUV[0]; if (my_abs(fL) >= RADIUS) continue;
				fU = color.val[1] - pLUV[1]; if (my_abs(fU) >= RADIUS) continue;
				fV = color.val[2] - pLUV[2]; if (my_abs(fV) >= RADIUS) continue;
				RAD2 = fL * fL + fU * fU + fV * fV;
				if (RAD2 < min_RAD2)
				{
					min_RAD2 = RAD2;
					min_ind = i;
				}
			}
			if (min_RAD2<RADIUS2) _labels[k] = min_ind;
			else                 _labels[k] = n_rects;

			k++;
		}
	}

	if (auto_segm) set_RADIUS(0, 1);
	else           set_RADIUS(0, 0);

	// SIYUAN modified
	k = 0;
	for (int r = 0; r < nRows; r++){
		for (int c = 0; c < nCols; c++){
			Vec3s color = mImg_LUV.at<Vec3s>(r, c);
			if (_labels[k] == n_rects){
				for (i = 0; i < 8; i++)
				{
					u = k + my_neigh[i];
					if ((u >= 0 && u < count) && (pres_class = _labels[u]) != n_rects)
					{
						pLUV = T[pres_class];
						fL = color.val[0] - pLUV[0]; if (my_abs(fL) >= RADIUS) continue;
						fU = color.val[1] - pLUV[1]; if (my_abs(fU) >= RADIUS) continue;
						fV = color.val[2] - pLUV[2]; if (my_abs(fV) >= RADIUS) continue;
						RAD2 = fL * fL + fU * fU + fV * fV;
						if (RAD2 < RADIUS2) _labels[k] = pres_class;
					}
				}
			}

			k++;
		}
	}
}

// Final allocation
void DDMCMC_MeanShiftClustering::get_codeblock(double T[][p_max], int n_rects)
{
	double	RAD2, min_RAD2;
	int		min_ind;
	int		i, k;
	double	*ptr;

	// SIYUAN modified
	k = 0;
	for (int r = 0; r < nRows; r++){
		for (int c = 0; c < nCols; c++){
			min_RAD2 = BIG_NUM;
			min_ind = 0;
			for (i = 0; i < n_rects; i++)
			{
				ptr = T[i];
				RAD2 = color_dis(r, c, ptr[0], ptr[1], ptr[2]);
				if (RAD2 < min_RAD2)
				{
					min_RAD2 = RAD2;
					min_ind = i;
				}
			}
			_labels[k] = min_ind;

			k++;
		}
	}
}

// Compute the mean of feature vectors mapped into the same color
void DDMCMC_MeanShiftClustering::new_codebook(double T[][p_max], int n_rects)
{
	int		i, k;
	int		*tab_contor = new int[n_rects];
	int		prez_class;
	double	*ptr;

	memset(tab_contor, 0, sizeof(int)*n_rects);
	for (i = 0; i < n_rects; i++)
	{
		T[i][0] = 0.0;
		T[i][1] = 0.0;
		T[i][2] = 0.0;
	}
	// SIYUAN modified
	k = 0;
	for (int r = 0; r < nRows; r++){
		for (int c = 0; c < nCols; c++){
			if ((prez_class = _labels[k]) != n_rects)
			{
				ptr = T[prez_class];
				Vec3s color = mImg_LUV.at<Vec3s>(r, c);
				ptr[0] += color.val[0];
				ptr[1] += color.val[1];
				ptr[2] += color.val[2];
				tab_contor[prez_class]++;
			}

			k++;
		}
	}

	for (i = 0; i < n_rects; i++)
	{
		T[i][0] /= tab_contor[i];
		T[i][1] /= tab_contor[i];
		T[i][2] /= tab_contor[i];
	}
	delete[] tab_contor;
}

// Determine the final feature palette
void DDMCMC_MeanShiftClustering::optimize(double T[][p_max], int n_rects)
{
	get_codeblock1(T, n_rects);
	new_codebook(T, n_rects);
	if (auto_segm)
		get_codeblock(T, n_rects);
	//cerr << ":";
}


// Inverse of the mapping array used in color elimination
void DDMCMC_MeanShiftClustering::reverse_map(uchar *inv_map, uchar *my_map, int *n_rects, uchar *valid_class, double T[][p_max])
{
	double	sec_T[Max_rects][p_max];
	int		u = 0, k, j;

	for (j = 0; j < *n_rects; j++)
	{
		if (valid_class[j])
		{
			for (k = 0; k<3; k++)
				sec_T[u][k] = T[j][k];
			my_map[j] = u;
			inv_map[u] = j;
			u++;
		}
	}
	//if (*n_rects < Max_rects) { // Modified by Jiadi Yang
		my_map[*n_rects] = u;
		inv_map[u] = *n_rects;
		*n_rects = u;
	//}
		for (j = 0; j < *n_rects; j++){
			for (k = 0; k < 3; k++){
				T[j][k] = sec_T[j][k];
			}
		}
}

// Eliminate colors that have less than "my_lim" connected pixels
void DDMCMC_MeanShiftClustering::eliminate_class(uchar *my_class, int *my_max_region, int *n_rects, int my_lim, uchar* inv_map, double T[][p_max], REGION *first_region)
{
	int		j, k;
	uchar	*valid_class;
	REGION	*current_region = first_region;

	valid_class = new uchar[*n_rects];
	for (j = 0; j < *n_rects; j++)
	{
		if (my_max_region[j]<my_lim) valid_class[j] = 0;
		else                        valid_class[j] = 1;
	}
	while (1)
	{
		if ((current_region->my_class < *n_rects && !valid_class[current_region->my_class]))
		for (k = 0; k < current_region->my_contor; k++)
			_labels[current_region->my_region[k]] = *n_rects;
		if (current_region->next_region_str)
			current_region = current_region->next_region_str;
		else break;
	}

	uchar my_map[Max_rects];

	reverse_map(inv_map, my_map, n_rects, valid_class, T);
	for (k = 0; k < count; k++)
		my_class[k] = my_map[_labels[k]];
	delete[] valid_class;
	memcpy(_labels, my_class, count);
}

// Eliminate regions with less than "my_lim" pixels
void DDMCMC_MeanShiftClustering::eliminate_region(int *n_rects, int my_lim, double T[][p_max], REGION* first_region)
{
	int		j, u, k, p, pres_class, min_ind;
	REGION	*current_region = first_region;
	int		*region;
	double	*ptr;
	double	RAD2, minRAD2;
	int		increm;

	while (1)
	{
		if (current_region->my_contor < my_lim)
		{
			set_RADIUS(0, 0);
			increm = 4;
			region = current_region->my_region;
			for (k = 0; k < current_region->my_contor; k++)
				_labels[region[k]] = *n_rects;
			while (1)
			{
				bool my_flag = 0;
				RADIUS += increm; RADIUS2 = RADIUS * RADIUS; increm += 4;
				for (k = 0; k < current_region->my_contor; k++)
				if (_labels[p = region[k]] == (*n_rects))
				{
					minRAD2 = RADIUS2;
					for (j = 1; j < 8; j += 2)
					{
						u = p + my_neigh[j];
						if ((u >= 0 && u < count) && (pres_class = _labels[u]) != (*n_rects))
						{
							ptr = T[pres_class];
							RAD2 = color_dis(p, ptr[0], ptr[1], ptr[2]);
							if (RAD2 < minRAD2)
							{
								minRAD2 = RAD2;
								min_ind = pres_class;
							}
						}
					}
					if (minRAD2 < RADIUS2) _labels[p] = min_ind;
					my_flag = 1;
				}
				if (!my_flag) break;
			}
		}

		if (current_region->next_region_str)
			current_region = current_region->next_region_str;
		else break;
	}
}

// Destroy the region list
void DDMCMC_MeanShiftClustering::destroy_region_list(REGION *first_region)
{
	REGION *current_region = first_region;
	while (1)
	{
		delete[] current_region->my_region;
		first_region = current_region;
		if (current_region->next_region_str)
		{
			current_region = current_region->next_region_str;
			delete first_region;
		}
		else
		{
			delete first_region;
			break;
		}
	}
}

// Connected component main routine
void DDMCMC_MeanShiftClustering::find_other_neigh(int k, int *my_ptr_tab, REGION *current_region)
{
	int *ptr_tab = my_ptr_tab;
	int i, u, j = k, sec_signal;
	int contor = 0;
	int region_contor = current_region->my_contor;
	int region_class = current_region->my_class;

	ptr_tab[contor] = j;

	while (1)
	{
		sec_signal = 0;
		for (i = 1; i < 9; i += 2)
		{
			u = j + my_neigh[i];
			if ((u >= 0 && u < count) && _labels[u] == region_class && !taken[u])
			{
				sec_signal = 1;
				conn_selected[region_contor++] = u;
				taken[u] = 1;
				ptr_tab[++contor] = u;
			}
		}
		if (sec_signal) j = ptr_tab[contor];
		else
		{
			if (contor>1) j = ptr_tab[--contor];
			else break;
		}
	}
	current_region->my_contor = region_contor;
}

// Create the region list
REGION *DDMCMC_MeanShiftClustering::create_region_list(int *my_max_region, int change_type)
{
	int k, local_label = 0;
	REGION *first_region, *prev_region, *current_region;

	taken = new uchar[count];
	memset(taken, 0, count);
	conn_selected = new int[count];
	int *ptr_tab = new int[count];

	// SIYUAN
	current_region = new REGION;
	prev_region = new REGION;
	first_region = new REGION;

	for (k = 0; k<count; k++)
	if (!taken[k])
	{
		current_region = new REGION;
		current_region->my_contor = 0;
		current_region->my_class = _labels[k];
		current_region->my_label = local_label;
		if (k != 0) prev_region->next_region_str = current_region;
		else		first_region = current_region;

		local_label++;
		conn_selected[current_region->my_contor++] = k;
		taken[k] = 1;
		find_other_neigh(k, ptr_tab, current_region);
		if (change_type == 0 && my_max_region[current_region->my_class]<current_region->my_contor)
			my_max_region[current_region->my_class] = current_region->my_contor;
		current_region->my_region = new int[current_region->my_contor];

		memcpy(current_region->my_region, conn_selected, sizeof(int)*current_region->my_contor);
		prev_region = current_region;
	}
	current_region->next_region_str = 0;

	delete[] ptr_tab; delete[] taken; delete[] conn_selected;
	return first_region;
}

// Find connected components and remove small regions of classes
// with small regions
void DDMCMC_MeanShiftClustering::conn_comp(uchar *my_class, int *n_rects, uchar *inv_map, double T[][p_max], int my_lim, int change_type)
{
	REGION *first_region = NULL;
	int *my_max_region = NULL;
	if (change_type == 0)
	{
		my_max_region = new int[(*n_rects) + 1];
		memset(my_max_region, 0, sizeof(int)*((*n_rects) + 1));
	}
	first_region = create_region_list(my_max_region, change_type);
	if (change_type == 0)  //elliminate classes with small regions
		eliminate_class(my_class, my_max_region, n_rects, my_lim, inv_map, T, first_region);
	else if (change_type == 1)  //elliminate small regions
		eliminate_region(n_rects, my_lim, T, first_region);
	destroy_region_list(first_region);
	if (change_type == 0) delete[] my_max_region;
	//cerr << ":";
}


// Cut a rectangle from the entire input data
// Deletes the previous rectangle, if any
void DDMCMC_MeanShiftClustering::cut_rectangle(Rect* rect)
{
	if (_data)
	{
		for (int i = 0; i < _k; i++)
		if (_data[i])
			delete[] _data[i];
		delete[] _data;
	}

	// Set the dimensions of the currently processed region.
	_rg_rows = rect->height;
	_rg_colms = rect->width;
	_data = new int*[_k];

	int my_x = rect->x;
	int my_y = rect->y;
	int i, j, d;
	for (i = 0; i < _k; i++)
		_data[i] = new int[_rg_colms*_rg_rows];

	if (auto_segm)
	{
		// SIYUAN modified
			// memcpy(_data[d], &(LUV[d](0)), sizeof(int)*count);
		int k = 0;
		for (int r = 0; r < nRows; r++){
			for (int c = 0; c < nCols; c++){
				Vec3s color = mImg_LUV.at<Vec3s>(r, c);
				_data[0][k] = color.val[0];
				_data[1][k] = color.val[1];
				_data[2][k] = color.val[2];

				// SIYUAN testing
				/*
				if ((r == 39 || r == 102 || r == 291) && (c == 255 || c == 276 || c == 290)){
					cout << r << " " << c << " " << r*nCols + c << " " << d << " "  << k << endl;
					cout << _data[d][k] << " " << (int)mImg_LUV.at<Vec3s>(r, c).val[0] << endl;
					cout << _data[d][k] << " " << (int)mImg_LUV.at<Vec3s>(r, c).val[1] << endl;
					cout << _data[d][k] << " " << (int)mImg_LUV.at<Vec3s>(r, c).val[2] << endl << endl;
				}
				*/
				k++;
			}
		}
	}
	else
	{
		int	idx1 = my_y * nCols + my_x;
		int	idx2 = 0;
		for (j = my_y, d; j < my_y + _rg_rows; j++, idx1 += nCols - _rg_colms)
		for (i = my_x; i < my_x + _rg_colms; i++, idx1++, idx2++)
		{
			for (d = 0; d < _k; d++){
				// SIYUAN modified
				// _data[d][idx2] = LUV[d](idx1);
				int r = idx1 / nCols;
				int c = idx1 % nCols;
				_data[d][idx2] = mImg_LUV.at<Vec3s>(r, c).val[d];
			}
		}
	}
	//cerr << ":";
}

// Compute the mean of N points given by J[]
void DDMCMC_MeanShiftClustering::mean_s(const int N, const int p, int J[], int **data, double T[])
{
	int TT[p_max];
	int k, i, j;

	for (i = 0; i < p; i++)
		TT[i] = 0;
	for (i = 0; i < N; i++)
	{
		k = J[i];
		for (j = 0; j < p; j++)
			TT[j] += data[j][k];
	}
	for (i = 0; i < p; i++)
		T[i] = (double)TT[i] / (double)N;
}

// Build a subsample set of 9 points
int DDMCMC_MeanShiftClustering::subsample(double *Xmean)
{
	int J[9];
	int my_contor = 0, uj, i0;

	if (auto_segm)
		i0 = J[my_contor] = _remn_cnt_map[int(double(_remn_cnt)*double(rand()) / double(SHRT_MAX))];
	else
		i0 = J[my_contor] = int(double(_rg_cnt)*double(rand()) / double(SHRT_MAX));
	my_contor++;

	for (int i = 0; i < 8; i++)
	{
		uj = i0 + my_neigh_r[i];
		if (uj >= 0 && uj < _rg_cnt)
		{
			if (auto_segm && _labels[uj] != 255)
				break;
			else
			{
				J[my_contor] = uj;
				my_contor++;
			}
		}
	}
	mean_s(my_contor, _k, J, _data, Xmean);
	return 1;
}

// Sampling routine with all needed tests
// randomly choose a high density region
double DDMCMC_MeanShiftClustering::my_sampling(int rect, double T[Max_rects][p_max])
{
	int		k, c;
	double	fL, fU, fV;
	double	my_dist = max_dist, my_sqrt_dist = fix_RADIUS[0];
	double	TJ[Max_J][p_max];
	int		l = 0;       //contor of number of subsample sets
	int		ll = 0;       //contor of trials
	double	Xmean[p_max];
	double	Obj_fct[Max_J];

	//Max_trials = max number of failed trials
	//_max_sub_samp = max number of subsample sets
	// computation of numbers that are lying within circle of sample means
	while ((ll < Max_trials) && (l < _max_sub_samp))
	{
		if (subsample(Xmean))    // the subsample procedure succeeded
		{
			ll = 0; c = 0;

			// Save the mean
			for (k = 0; k < _k; k++) TJ[l][k] = Xmean[k];

			// Compute the square residuals (Euclid dist.)
			if (auto_segm)
			for (int p = 0; p < _n_col_remain; p++)
			{
				k = _col_remain[p];
				fL = _col0[k] - Xmean[0]; if (my_abs(fL) >= my_sqrt_dist) continue;
				fU = _col1[k] - Xmean[1]; if (my_abs(fU) >= my_sqrt_dist) continue;
				fV = _col2[k] - Xmean[2]; if (my_abs(fV) >= my_sqrt_dist) continue;
				if (fL*fL + fU*fU + fV*fV<my_dist) c += _m_col_remain[k];
			}
			else
			for (k = 0; k < _rg_cnt; k++)
			{
				fL = _data[0][k] - Xmean[0];
				if (my_abs(fL) >= my_sqrt_dist) continue;
				fU = _data[1][k] - Xmean[1];
				if (my_abs(fU) >= my_sqrt_dist) continue;
				fV = _data[2][k] - Xmean[2];
				if (my_abs(fV) >= my_sqrt_dist) continue;
				if (fL*fL + fU*fU + fV*fV<my_dist) c++;
			}

			//  Objective functions
			Obj_fct[l] = c;
			l++;
		}
		else ++ll;
	}
	if (ll == Max_trials && l < 1) return(BIG_NUM); // Cannot find a kernel

	//  Choose the highest density
	fL = -BIG_NUM; c = 0;
	for (k = 0; k < _max_sub_samp; k++)
	if (Obj_fct[k] > fL)
	{
		fL = Obj_fct[k];
		c = k;
	}
	if (Obj_fct[c]>0)
	for (k = 0; k<_k; k++)
		T[rect][k] = TJ[c][k];
	else return -BIG_NUM; // Not enough points
	return (0);
}

// Compute the weighted covariance of N points
void DDMCMC_MeanShiftClustering::covariance_w(const int N, int M, const int p, int **data,
	int *w, double T[], double C[p_max][p_max])
{
	int i, j, k, l;
	int TT[p_max];

	// calculation of mean value
	for (i = 0; i < p; i++)
		TT[i] = 0;
	for (i = 0; i < M; i++)
	for (j = 0; j < p; j++)
		TT[j] += w[i] * data[j][i];
	for (i = 0; i < p; i++)
		T[i] = (double)TT[i] / (double)N;

	for (i = 0; i < p; i++)
	for (j = i; j < p; j++)
		C[i][j] = 0.0;
	for (i = 0; i < M; i++)
	{
		for (k = 0; k < p; k++)
		for (l = k; l < p; l++)
			C[k][l] += w[i] * (data[k][i] - T[k]) * (data[l][i] - T[l]);
	}
	for (k = 0; k < p; k++)
	{
		for (l = k; l < p; l++)
			C[k][l] /= (double)(N - 1);
		for (l = 0; l < k; l++)
			C[k][l] = C[l][k];
	}
}

// neighbor initialization
void DDMCMC_MeanShiftClustering::init_neigh(void)
{
	my_neigh[0] = -nCols - 1;  my_neigh[1] = -nCols;
	my_neigh[2] = -nCols + 1;  my_neigh[3] = +1;
	my_neigh[4] = +nCols + 1;  my_neigh[5] = +nCols;
	my_neigh[6] = +nCols - 1;  my_neigh[7] = -1;

	my_neigh_r[0] = -_rg_colms - 1;  my_neigh_r[1] = -_rg_colms;
	my_neigh_r[2] = -_rg_colms + 1;  my_neigh_r[3] = +1;
	my_neigh_r[4] = +_rg_colms + 1;  my_neigh_r[5] = +_rg_colms;
	my_neigh_r[6] = +_rg_colms - 1;  my_neigh_r[7] = -1;
}

// Init matrices parameters
void DDMCMC_MeanShiftClustering::init_matr(void)
{
	// General statistic parameters for X.
	double	Mg[p_max];         //sample mean of X
	double	C[p_max][p_max];   //sample covariance matrix of X
	covariance_w(count, _n_colors, _k, _col_all, _m_colors, Mg, C);

	// Adaptation
	double my_th = C[0][0] + C[1][1] + C[2][2];
	int active_gen_contor = 1;

	if (auto_segm)
		fix_RADIUS[0] = gen_RADIUS[option] * sqrt(my_th / 100);
	else
	{
		active_gen_contor = rect_gen_contor;
		for (int i = 0; i<active_gen_contor; i++)
			fix_RADIUS[i] = rect_RADIUS[i] * sqrt(my_th / 100);
	}
	final_RADIUS = fix_RADIUS[active_gen_contor - 1] * 1.26;
	max_dist = fix_RADIUS[0] * fix_RADIUS[0];
#ifdef TRACE
	printf("\n %.2f %.2f ", fix_RADIUS[0], final_RADIUS);
#endif
	act_threshold = (int)((my_threshold[option]>sqrt((float)count) / my_rap[option]) ?
		my_threshold[option] : sqrt((float)count) / my_rap[option]);
	//cerr << ":";
}

// Init
void DDMCMC_MeanShiftClustering::initializations(Rect rects[], int *n_rects, int selects, int *active_gen_contor)
{
	int i;

	CreateLUVspace();

	// neighborhood initialization
	init_neigh();

	_labels = new uchar[count];
	memset(_labels, 255, count);
	if (!(*n_rects))
	{
		auto_segm = 1;
		*n_rects = Max_rects - 1; // Siyuan
		_remn_cnt = count;
		_n_col_remain = _n_colors;

		_remn_cnt_map = new int[count];
		_col_remain = new int[_n_colors];
		_m_col_remain = new int[_n_colors];

		for (i = 0; i< count; i++)
			_remn_cnt_map[i] = i;
		for (i = 0; i< _n_colors; i++)
			_col_remain[i] = i;
		memcpy(_m_col_remain, _m_colors, sizeof(int)*_n_colors);

		for (i = 0; i < Max_rects; i++)
		{
			rects[i].width = nCols; rects[i].height = nRows;
			rects[i].x = 0;        rects[i].y = 0;
		}
		*active_gen_contor = 1;
	}
	else
	{
		auto_segm = 0;
		_remn_cnt = 0;
		*active_gen_contor = rect_gen_contor;
		option = 2;
	}
	init_matr();
	delete[] _m_colors;
	_m_colors = NULL;
}

// Mean shift segmentation applied on the selected region(s) of an image or
// on the whole image
bool DDMCMC_MeanShiftClustering::ms_segment(Rect rects[], int n_rec, int selects,
	unsigned int seed_default, bool block,
	int nOption)
{
	int	n_rects = n_rec;

	option = nOption;

	//int t = time(0);
	if (n_rects >= Max_rects) return false; // Siyuan
	if (selects & Lightness || selects & Ustar || selects & Vstar)
		_k = 3;
	else return false;

	int		contor_trials = 0, active_gen_contor;
	double	T[Max_rects][p_max];
	int		TI[Max_rects][p_max];
	double	fL, fU, fV, RAD2, q;
	int		i, k;

	srand(seed_default);
	initializations(rects, &n_rects, selects, &active_gen_contor);

	// Mean shift algorithm and removal of the detected feature
	int rect;
	for (rect = 0; rect < n_rects; rect++)
	{
		_rg_cnt = rects[rect].width * rects[rect].height;
		cut_rectangle(&rects[rect]);

	MS:
		if (auto_segm && contor_trials) _max_sub_samp = 1;

		q = my_sampling(rect, T);
		if (q == -BIG_NUM || q == BIG_NUM)
		if (contor_trials++<MAX_TRIAL)  goto MS;
		else                           break;

		double final_T[p_max];
		for (i = 0; i < _k; i++)
			final_T[i] = T[rect][i];

		int *selected = new int[count];
		uchar *sel_col = new uchar[_n_colors];
		uchar *my_class = new uchar[count];

		int my_contor = 0, color_cnt = -1, how_many = 10;
		while (color_cnt++ < active_gen_contor - 1)
		{
			set_RADIUS(color_cnt, 0);
			int gen_contor = 0;
			bool last_loop = 0;

			while (gen_contor++<how_many)
			{
				if (auto_segm)
				{
					memset(sel_col, 0, _n_colors);
					// execuation of mean shift algorightm
					new_auto_loop(final_T, sel_col);
					fL = T[rect][0] - final_T[0];
					fU = T[rect][1] - final_T[1];
					fV = T[rect][2] - final_T[2];
					RAD2 = fL * fL + fU * fU + fV * fV;
					if (RAD2<0.1)
					{
						my_contor = 0;
						memset(my_class, 0, count);
						for (k = 0; k < _remn_cnt; k++)
						{
							int p = _remn_cnt_map[k];
							if (sel_col[_col_index[p]])
							{
								selected[my_contor++] = p;
								my_class[p] = 1;
							}
						}
						break;
					}
					else
					{
						T[rect][0] = final_T[0];
						T[rect][1] = final_T[1];
						T[rect][2] = final_T[2];
					}
				}
				else
				{
					my_contor = 0;
					memset(my_class, 0, count);
					nauto_loop(final_T, selected, my_class, &my_contor);
					mean_s(my_contor, _k, selected, _data, final_T);
					fL = T[rect][0] - final_T[0]; fU = T[rect][1] - final_T[1];
					fV = T[rect][2] - final_T[2]; RAD2 = fL*fL + fU*fU + fV*fV;

					if (RAD2 < 0.1)
						break;
					else
					{
						T[rect][0] = final_T[0];
						T[rect][1] = final_T[1];
						T[rect][2] = final_T[2];
					}
				}
			}
		}
		if (auto_segm)
		{
			if (option == 0) test_neigh(my_class, selected, &my_contor, 2);
			else if (option == 1) test_neigh(my_class, selected, &my_contor, 1);
			else if (option == 2) test_neigh(my_class, selected, &my_contor, 0);
		}
#ifdef TRACE
		printf("my_contor = %d contor_trials = %d", my_contor, contor_trials);
#endif
		if (!auto_segm && test_same_cluster(rect, T))
		{
			delete[] my_class;
			delete[] sel_col;
			delete[] selected;
			continue;
		}
		if (auto_segm && my_contor < act_threshold)
		{
			delete[] my_class;
			delete[] sel_col;
			delete[] selected;
			if (contor_trials++ < MAX_TRIAL) goto MS;
			else                          break;
		}

		if (auto_segm) my_actual(my_class);

		convert_LUV_RGB(final_T, TI[rect], selects);
		for (k = 0; k < count; k++){
			if (my_class[k]){
					_labels[k] = rect;
			}
		}
		delete[] my_class; delete[] sel_col; delete[] selected;
	}
	n_rects = rect;
	uchar** isegment = new uchar*[3];
	int j;
	for (j = 0; j < 3; j++)
	{
		isegment[j] = new uchar[count];
		memset(isegment[j], 0, count);
	}
	uchar *isegment0 = isegment[0];
	uchar *isegment1 = isegment[1];
	uchar *isegment2 = isegment[2];

	if (auto_segm)
	{
		uchar *my_class = new uchar[count];
		for (int k = 0; k < count; k++){
			if (_labels[k] == 255){
				_labels[k] = n_rects;
			}
		}

		uchar inv_map[Max_rects];
		for (int k = 0; k < n_rects; k++){
			inv_map[k] = k;
		}

		if (option == 0)  conn_comp(my_class, &n_rects, inv_map, T, 10, 0);
		else if (option == 1) conn_comp(my_class, &n_rects, inv_map, T, 20, 0);
		else conn_comp(my_class, &n_rects, inv_map, T, act_threshold, 0);
		optimize(T, n_rects);
		for (int k = 0; k < n_rects; k++) convert_LUV_RGB(T[k], TI[k], selects);

		// Postprocessing
		if (option == 1 || option == 2)
		for (int k = 2; k < 10; k += 2) conn_comp(my_class, &n_rects, inv_map, T, k, 1);
#ifdef TRACE
		printf("\nTime = %d seconds\n", time(0) - t);
#endif
		uchar my_max_ind;
		// SIYUAN modified
		ImageLabels = Mat(nRows, nCols, CV_8U);
		int k = 0;
		for (int r = 0; r < nRows; r++){
			for (int c = 0; c < nCols; c++){
				if ((my_max_ind = _labels[k]) != n_rects)
				{
					int *ti = TI[my_max_ind];
					isegment0[k] = ti[0]; isegment1[k] = ti[1]; isegment2[k] = ti[2];
					ImageLabels.at<uchar>(r, c) = my_max_ind;
				}

				k++;
			}
		}

		delete[] my_class;
	}
	else   // if not auto_segmentation
	{
		optimize(T, n_rects);
		for (k = 0; k < n_rects; k++)
			convert_LUV_RGB(T[k], TI[k], selects);
		int temp_class;
		// SIYUAN modified
		int k = 0;
		for (int r = 0; r < nRows; r++){
			for (int c = 0; c < nCols; c++){
				if ((temp_class = _labels[k])<n_rects)
				{
					Vec3b color = mImg_RGB.at<Vec3b>(r, c);
					isegment0[k] = color.val[0];
					isegment1[k] = color.val[1];
					isegment2[k] = color.val[2];
				}

				k++;
			}
		}
	}
	if (auto_segm)
	{
		delete[] _m_col_remain;
		delete[] _col_remain;
		delete[] _remn_cnt_map;
	}
	delete[] _labels;		_labels = NULL;
	delete[] _col_index;	_col_index = NULL;
	delete[] _col0;		_col0 = NULL;
	delete[] _col1;		_col1 = NULL;
	delete[] _col2;		_col2 = NULL;
	delete[] _col_all;		_col_all = NULL;

	// SIYUAN modified
	int mm = 0;
	for (int r = 0; r < nRows; r++){
		for (int c = 0; c < nCols; c++){
			mImg_RGB.at<Vec3b>(r, c).val[0] = isegment[0][mm];
			mImg_RGB.at<Vec3b>(r, c).val[1] = isegment[1][mm];
			mImg_RGB.at<Vec3b>(r, c).val[2] = isegment[2][mm];

			mm++;
		}
	}

	//cerr << endl << "Original Colors: " << _n_colors << endl;
	//cerr << "Segment. Colors: " << n_rects << endl;

	return true;
}


// Important helper functions
double DDMCMC_MeanShiftClustering::color_dis(const int row, const int col, const double mean_L,
	const double mean_U, const double mean_V)
{
	double fL, fU, fV;
	Vec3s color = mImg_LUV.at<Vec3s>(row, col);

	fL = color.val[0] - mean_L;
	fU = color.val[1] - mean_U;
	fV = color.val[2] - mean_V;
	return (fL * fL + fU * fU + fV * fV);
}

double DDMCMC_MeanShiftClustering::color_dis(const int index, const double mean_L,
	const double mean_U, const double mean_V)
{
	int row = index / nCols, col = index % nCols;
	double fL, fU, fV;
	Vec3s color = mImg_LUV.at<Vec3s>(row, col);

	fL = color.val[0] - mean_L;
	fU = color.val[1] - mean_U;
	fV = color.val[2] - mean_V;
	return (fL * fL + fU * fU + fV * fV);
}


int	DDMCMC_MeanShiftClustering::ShiftColorValues(const int index){
	int x = index % nCols;
	int y = index / nCols;
	Vec3b color = mImg_RGB.at<Vec3b>(y, x);
	int R = color.val[0], G = color.val[1], B = color.val[2];
	return (((((int)R) << 8) + (int)G) << 8) + (int)B;
}


void DDMCMC_MeanShiftClustering::convert_RGB_LUV(int selects)
{
	int		x, y, z, my_temp;
	double	l_star, u_star, v_star;
	double	u_prime, v_prime;
	int temp_col, temp_index, temp_ind;
	int j, k;

	int a00 = RGB_LUV_XYZ[0][0], a01 = RGB_LUV_XYZ[0][1], a02 = RGB_LUV_XYZ[0][2];
	int a10 = RGB_LUV_XYZ[1][0], a11 = RGB_LUV_XYZ[1][1], a12 = RGB_LUV_XYZ[1][2];
	int a20 = RGB_LUV_XYZ[2][0], a21 = RGB_LUV_XYZ[2][1], a22 = RGB_LUV_XYZ[2][2];

	int *A00 = new int[MAXV]; int *A01 = new int[MAXV]; int *A02 = new int[MAXV];
	int *A10 = new int[MAXV]; int *A11 = new int[MAXV]; int *A12 = new int[MAXV];
	int *A20 = new int[MAXV]; int *A21 = new int[MAXV]; int *A22 = new int[MAXV];

	for (j = 0; j<MAXV; j++)
	{
		A00[j] = a00 * j; A01[j] = a01 * j; A02[j] = a02 * j;
		A10[j] = a10 * j; A11[j] = a11 * j; A12[j] = a12 * j;
		A20[j] = a20 * j; A21[j] = a21 * j; A22[j] = a22 * j;
	}

	double *my_pow = new double[MAXV];
	for (j = 0; j < MAXV; j++)
		my_pow[j] = (double)(116.0 * pow(j / 255.0, 0.3333333) - 16);

	for (j = 0; j < _n_colors; j++)
	{
		temp_col = _col_RGB[j];
		int R = temp_col >> 16;
		int G = (temp_col >> 8) & 255;
		int B = temp_col & 255;

		x = A00[R] + A01[G] + A02[B];
		y = A10[R] + A11[G] + A12[B];
		z = A20[R] + A21[G] + A22[B];

		double  tval = (double)(y / 2550000.0); //Yn==1
		if (tval >  Lt)
			l_star = my_pow[(int)(tval * 255 + 0.5)];
		else
			l_star = 903.3 * tval;

		my_temp = x + 15 * y + 3 * z;
		if (my_temp)
		{
			u_prime = (double)(x << 2) / (double)(my_temp);
			v_prime = (double)(9 * y) / (double)(my_temp);
		}
		else
		{
			u_prime = 4.0;
			v_prime = 9.0 / 15.0;
		}

		tval = 13 * l_star;
		u_star = tval * (u_prime - Un_prime); // Un_prime = 0.1978
		v_star = tval * (v_prime - Vn_prime); // Vn_prime = 0.4683

		_col0[j] = (int)(l_star + 0.5);
		if (u_star>0)
			_col1[j] = (int)(u_star + 0.5);
		else
			_col1[j] = (int)(u_star - 0.5);

		if (v_star>0)
			_col2[j] = (int)(v_star + 0.5);
		else
			_col2[j] = (int)(v_star - 0.5);
	}

	// SIYUAN modified
	j = 0;
	for (int r = 0; r < nRows; r++){
		for (int c = 0; c < nCols; c++){
			temp_col = ShiftColorValues(j);
			temp_ind = _col_misc[temp_col >> 6];
			for (k = temp_ind; k < temp_ind + SEC_SIZE; k++)
				if (_col_RGB[k] == temp_col)
				{
					temp_index = _col_index[j] = k;
					break;
				}

			mImg_LUV.at<Vec3s>(r, c).val[0] = _col0[temp_index];
			mImg_LUV.at<Vec3s>(r, c).val[1] = _col1[temp_index];
			mImg_LUV.at<Vec3s>(r, c).val[2] = _col2[temp_index];

			// SIYUAN testing
			/*
			if ((r == 39 || r == 102 || r==291) && (c == 255 || c == 276 || c == 290)){
			cout << r << " " << c << " " << r*nCols + c << endl;
			cout << (int)mImg_LUV.at<Vec3s>(r, c).val[0] << endl;
			cout << (int)mImg_LUV.at<Vec3s>(r, c).val[1] << endl;
			cout << (int)mImg_LUV.at<Vec3s>(r, c).val[2] << endl << endl;
			}
			*/

			j++;
		}
	}

	delete[] my_pow;
	delete[] A22; delete[] A21; delete[] A20;
	delete[] A12; delete[] A11; delete[] A10;
	delete[] A02; delete[] A01; delete[] A00;
	//cerr << ":";
}

void DDMCMC_MeanShiftClustering::convert_LUV_RGB(double *pLUV, int *pRGB, int selects)
{
	int		i;
	double	R_G_B[3][3] = { { (double)3.2405, (double)-1.5371, (double)-0.4985 },
	{ (double)-0.9693, (double)1.8760, (double)0.0416 },
	{ (double)0.0556, (double)-0.2040, (double)1.0573 } };

	// this condition is always true
	if (selects & Lightness && selects & Ustar && selects & Vstar)
	{
		if (pLUV[0] < 0.1)
		{
			pRGB[0] = 0;
			pRGB[1] = 0;
			pRGB[2] = 0;
		}
		else
		{
			double my_x, my_y, my_z;
			if (pLUV[0] < 8.0)
				my_y = Yn * pLUV[0] / 903.3;
			else
				my_y = Yn * pow((pLUV[0] + 16.0) / 116.0, 3);

			double u_prime = pLUV[1] / (13 * pLUV[0]) + Un_prime;
			double v_prime = pLUV[2] / (13 * pLUV[0]) + Vn_prime;

			my_x = 9 * u_prime * my_y / (4 * v_prime);
			my_z = (12 - 3 * u_prime - 20 * v_prime) * my_y / (4 * v_prime);

			pRGB[0] = int((R_G_B[0][0] * my_x + R_G_B[0][1] * my_y + R_G_B[0][2] * my_z)*255.0);
			pRGB[1] = int((R_G_B[1][0] * my_x + R_G_B[1][1] * my_y + R_G_B[1][2] * my_z)*255.0);
			pRGB[2] = int((R_G_B[2][0] * my_x + R_G_B[2][1] * my_y + R_G_B[2][2] * my_z)*255.0);

			for (i = 0; i < 3; i++)
			{
				if (pRGB[i] > 255) pRGB[i] = 255;
				else if (pRGB[i] < 0) pRGB[i] = 0;
			}
		}
	}
	else
	{
		pRGB[0] = (int)pLUV[0];
		pRGB[1] = (int)pLUV[1];
		pRGB[2] = (int)pLUV[2];
	}
}


void DDMCMC_MeanShiftClustering::CreateLUVspace(void){
	histogram(); // get histogram of this image
	convert_RGB_LUV();
}

void DDMCMC_MeanShiftClustering::Free_histogram_space(){
	if (_col_all != NULL){
		delete[](_col_all[2]);
		delete[](_col_all[1]);
		delete[](_col_all[0]);
		delete[] _col_all;
	}
	if (_col_index != NULL)
		delete[] _col_index;
	if (_col_misc != NULL)
		delete[] _col_misc;
	if (_col_RGB != NULL)
		delete[] _col_RGB;
	if (_m_colors != NULL)
		delete[] _m_colors;
	_m_colors = NULL;
	_col_RGB = NULL;

	_col_misc = NULL;
	_col_index = NULL;
	_col_all = NULL;
}


void DDMCMC_MeanShiftClustering::histogram(int selects)
{
	// free assigned space
	Free_histogram_space();

	int *first_tab = new int[FIRST_SIZE];
	int **third_tab;
	int *fourth_tab;
	int *fifth_tab = new int[SEC_SIZE];
	uchar oR, oG, oB;

	_n_colors = 0;

	int k, j, p, r;
	int temp_ind, sec_ind, third_ind;

	int first_contor = 0, third_contor = 0;

	memset(first_tab, 0, sizeof(int)* FIRST_SIZE);
	_col_misc = new int[FIRST_SIZE];
	memset(_col_misc, 0, sizeof(int)* FIRST_SIZE);

	//first_tab -> how many
	// SIYUAN modified
	for (int i = 0; i < nRows; i++){
		for (int j = 0; j < nCols; j++){
			Vec3b color = mImg_RGB.at<Vec3b>(i, j);
			oR = (uchar)color.val[0];
			oG = (uchar)color.val[1];
			oB = (uchar)color.val[2];
			/*
			oR = (uchar)mImg_BGR.at<Vec3b>(i, j).val[2];
			oG = (uchar)mImg_BGR.at<Vec3b>(i, j).val[1];
			oB = (uchar)mImg_BGR.at<Vec3b>(i, j).val[0];
			*/

			temp_ind = (((oR << 8) + oG) << 2) + (oB >> 6);
			first_tab[temp_ind]++;
		}
	}
	//_col_misc -> memo position
	for (k = 0; k < FIRST_SIZE; k++)
		if (first_tab[k])
		{
			_col_misc[k] = first_contor;
			first_contor++;
		}
	//contors
	fourth_tab = new int[first_contor];
	memset(fourth_tab, 0, sizeof(int)* first_contor);
	//tab of pointers to reduced colors
	third_tab = new int *[first_contor];
	first_contor = 0;
	for (k = 0; k < FIRST_SIZE; k++)
		if (first_tab[k])
		{
			third_tab[first_contor] = new int[first_tab[k]];
			first_contor++;
		}

	// SIYUAN modified
	for (int i = 0; i < nRows; i++){
		for (int j = 0; j < nCols; j++){
			Vec3b color = mImg_RGB.at<Vec3b>(i, j);
			oR = (uchar)color.val[0];
			oG = (uchar)color.val[1];
			oB = (uchar)color.val[2];

			temp_ind = (((oR << 8) + oG) << 2) + (oB >> 6);
			sec_ind = oB & 63;
			third_ind = _col_misc[temp_ind];
			third_tab[third_ind][fourth_tab[third_ind]] = sec_ind;
			fourth_tab[third_ind]++;
		}
	}

	for (k = 0; k < first_contor; k++)
	{
		memset(fifth_tab, 0, sizeof(int)*SEC_SIZE);
		for (j = 0; j<fourth_tab[k]; j++)
			fifth_tab[third_tab[k][j]]++;
		for (j = 0; j < SEC_SIZE; j++)
			if (fifth_tab[j])
				_n_colors++;
	}

	_col_RGB = new int[_n_colors];
	_m_colors = new int[_n_colors];

	k = 0; p = 0;
	for (r = 0; r < FIRST_SIZE; r++)
		if (first_tab[r])
		{
			memset(fifth_tab, 0, sizeof(int)*SEC_SIZE);
			for (j = 0; j < fourth_tab[k]; j++)
				fifth_tab[third_tab[k][j]]++;
			_col_misc[r] = p;
			for (j = 0; j < SEC_SIZE; j++)
				if (fifth_tab[j])
				{
					_col_RGB[p] = (r << 6) + j;
					_m_colors[p] = fifth_tab[j];
					p++;
				}
			delete[] third_tab[k];
			k++;
		}

	delete[] third_tab;
	delete[] fourth_tab;
	delete[] fifth_tab;
	delete[] first_tab;

	_col_all = new int*[3];
	_col0 = _col_all[0] = new int[_n_colors];
	_col1 = _col_all[1] = new int[_n_colors];
	_col2 = _col_all[2] = new int[_n_colors];
	_col_index = new int[count];
	//cerr << ":";
}




// Corresponding to the original function "SegmentationMeanshift".
void DDMCMC_MeanShiftClustering::meanShiftSegmentation(int nOption){
	unsigned int	seed = 29254088; //random # generator
	int			selects = Lightness | Ustar | Vstar;
	int				n_rect = 0;
	Rect		rects[Max_rects];

	for (int i = 0; i < Max_rects; i++){
		rects[i] = Rect(0, 0, nCols, nRows);
	}

	ms_segment(rects, n_rect, selects, seed, false, nOption);

	mImg_LUV.convertTo(mImg_LUV_CV8UC3, CV_8UC3);

	/*
	Mat ms_clustering = getLabels();
	Mat image = ms_clustering;
	CopyPseudoColor(image);

	CopyFromRasterColor(*m_pDib, *((Raster_Color<uchar>*)pSeg));
	pSeg->CreateLUVspace();
	*/

	/*
	// Testing
	FileStorage fsTestFile1("D:/Works/Research/DDMCMC/Test/Siyuan/ms_clustering.txt", FileStorage::WRITE);
	fsTestFile1 << "ms_clustering" << ms_clustering;
	FileStorage fsTestFile2("D:/Works/Research/DDMCMC/Test/Siyuan/mImg_BGR.txt", FileStorage::WRITE);
	fsTestFile2 << "ms_clustering" << ms_clustering;
	imshow("mImg_BGR", mImg_BGR);
	mImg_LUV.convertTo(mImg_LUV, CV_8UC3);
	imshow("mImg_LUV", mImg_LUV);
	waitKey();
	*/
}
